/* coding by ivan wang to capture image and focus the image */
#include <netinet/in.h>
#include <arpa/inet.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/resource.h>
#include <sys/times.h>
#include <linux/videodev.h>
#include <linux/fb.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>

//#define VIDEO_MODE_422
#define LOG_CAP         //capture for display
#define VIDEO_PICTURE_QUEUE_SIZE_MAX 1
struct flcd_data
{
    unsigned int buf_len;
    unsigned int uv_offset;
    unsigned int frame_no;
	unsigned int mp4_map_dma[10];
};

#define FLCD_GET_DATA_SEP   0x46db
//#define FLCD_GET_DATA       0x46dc
#define FLCD_SET_FB_NUM     0x46dd
#define FLCD_SWITCH_MODE    0x46de
#define FLCD_SET_SPECIAL_FB    0x46df

#define FLCD_MODE_RGB       0
#define FLCD_MODE_YCBCR     1

//#define RGB_DISPLAY
int                 video_fd=-1;
struct video_mbuf   gb_buffers;
struct video_mmap   gb_buf;
unsigned char       *video_buf_virt;
int32_t RGB_Y_tab[256];
int32_t B_U_tab[256];
int32_t G_U_tab[256];
int32_t G_V_tab[256];
int32_t R_V_tab[256];

void show_lcd_352x240_l(unsigned char *fb_mem, unsigned char *yuv, int cap_width, int cap_height, int lcd_width, int lcd_height);

void
colorspace_init(void)
{
	int32_t i;
	for (i = 0; i < 16; i++) {
		RGB_Y_tab[i] = 128 * i;
		B_U_tab[i] = 222 * (16 - 128);
		G_U_tab[i] = 44 * (16 - 128);
		G_V_tab[i] = 88 * (16 - 128);
		R_V_tab[i] = 176 * (16 - 128);
	}
	for (i = 16; i < 240; i++) {
		RGB_Y_tab[i] = 128 * i;
		B_U_tab[i] = 222 * (i - 128);
		G_U_tab[i] = 44 * (i - 128);
		G_V_tab[i] = 88 * (i - 128);
		R_V_tab[i] = 176 * (i - 128);
	}
	for (i = 240; i < 256; i++) {
		RGB_Y_tab[i] = 128 * i;
		B_U_tab[i] = 222 * (240 - 128);
		G_U_tab[i] = 44 * (240 - 128);
		G_V_tab[i] = 88 * (240 - 128);
		R_V_tab[i] = 176 * (240 - 128);
	}
}

__inline int capture_trigger(struct video_mmap * vmap)
{
	return ioctl(video_fd, VIDIOCMCAPTURE, vmap);
}

__inline unsigned long capture_sync(int *gb_frame)
{
	return ioctl(video_fd, VIDIOCSYNC, gb_frame);
}

/* read a key without blocking */
static int read_key(void)
{
    int n = 1;
    unsigned char ch;
    struct timeval tv;
    fd_set rfds;

    FD_ZERO(&rfds);
    FD_SET(0, &rfds);
    tv.tv_sec = 0;
    tv.tv_usec = 0;
    n = select(1, &rfds, NULL, NULL, &tv);
    if (n > 0) {
        n = read(0, &ch, 1);
        if (n == 1)
            return ch;

        return n;
    }
    return -1;
}

void show_lcd(unsigned char *fb_mem, unsigned char *yuv, int cap_width, int cap_height, int lcd_width, int lcd_height);
void show_lcd_rgb(unsigned char *fb_mem, unsigned char *yuv, int cap_width, int cap_height, int lcd_width, int lcd_height);
void do_focus(int cap_width, int cap_height)
{
	struct video_capability video_cap;
	struct video_tuner tuner;
	int frame_sync;
	int cap_id;
	unsigned int length;
    struct video_window win;
    int fb,tmp;
	struct fb_var_screeninfo vinfo;
	void *fb_buf;
    unsigned char *fb_mem;
    int fb_mode;
    int key;
    struct flcd_data f_data;
    
	struct video_channel chan;
	int64_t cur_time;
     int type;
	// init LCD
    fb = open("/dev/fb0", O_RDWR);
#ifdef RGB_DISPLAY
  	fb_mode = FLCD_MODE_RGB;
#else
  	fb_mode = FLCD_MODE_YCBCR;
#endif
#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0))
	if (ioctl(fb,FLCD_SWITCH_MODE,&fb_mode) < 0) {
      printf("LCD Error: can not switch to RGB mode\n");
	}
#endif	
	if (ioctl(fb,FLCD_GET_DATA_SEP, &f_data) < 0) 
	{
    	printf("LCD Error: cannot get lcd data.\n");
    	exit(3);
	}
    	    	
    // Get variable screen information
    if (ioctl(fb, FBIOGET_VSCREENINFO, &vinfo)) {
        printf("LCD Error: cannot reading variable information.\n");
        exit(3);
    }    

    //fb_mem = ioremap((unsigned char *)f_data.frame_map_dma[1], f_data.buf_len);
    fb_buf=mmap(0,f_data.buf_len*f_data.frame_no,PROT_READ|PROT_WRITE,MAP_SHARED,fb, 0);
    if(fb_buf<0)
    {
        printf("Fail to allocate FLCD frame buffer\n");
        exit(3);
    }
    fb_mem=fb_buf;
    
	// init capture
	video_fd = open("/dev/video0", O_RDWR);
	if(video_fd<0)
	{
		/* open the second capture driver */
		video_fd = open("/dev/video1", O_RDWR);
		if(video_fd<0)
		{	/* all the capture are in use */
		    printf("open device failure");
		    return;
		}
		cap_id = 1;
		printf("open2 /dev/video1 complete!\n");
	}
	else{
		cap_id = 0;
		printf("open1 /dev/video0 complete!\n");
	}

    if (!ioctl(video_fd, VIDIOCGTUNER, &tuner))
	{
        tuner.mode = VIDEO_MODE_NTSC;
        ioctl(video_fd, VIDIOCSTUNER, &tuner);
    }

    colorspace_init();
    win.x = 0;
    win.y = 0;
    win.width = cap_width;
    win.height = cap_height;
    win.chromakey = -1;
    win.flags = 0; /* for set frame rate */

    if ( ioctl(video_fd,VIDIOCGMBUF,&gb_buffers) < 0 )
	{
		printf("capture not support mbuf\n");
		exit(0);
	}

    video_buf_virt = mmap(0, gb_buffers.size, PROT_READ|PROT_WRITE, MAP_SHARED, video_fd, 0);
    if ((unsigned char*)-1 == video_buf_virt)
    {
		printf("mmap error\n");
		exit(0);
    }
#if defined(LOG_CAP)
    type = 1;
    ioctl(video_fd, 0x5678, &type);
#else
    type = 0;
    ioctl(video_fd, 0x5678, &type);
#endif
    gb_buf.height = cap_height;
    gb_buf.width = cap_width;
#ifdef VIDEO_MODE_422
    gb_buf.format = VIDEO_PALETTE_YUV422;
#else    
    gb_buf.format = VIDEO_PALETTE_YUV420P;
#endif    

    fprintf(stderr, "Press [q] to stop previewing\n");

//++ Foster
	/*first trigger*/
//	if(capture_trigger(&gb_buf) < 0)
//		return;
//-- Foster

    length=0;
    
    tmp=1;    
	if (ioctl(fb,FLCD_SET_SPECIAL_FB,&tmp) < 0) 
    {
    	printf("LCD Error: cannot set frame buffer as %d.\n",tmp);
    	exit(3);
	}
	fb_mem+=(f_data.buf_len*tmp); //switch to 
	
    if (fb_mode != FLCD_MODE_RGB)
        memset (fb_mem, 0x1d, vinfo.xres * vinfo.yres); 
    while(1)
    {
        key = read_key();
        if (key == 'q') 
        {
           //memset (fb_mem, 0x1d, f_data.buf_len);
           tmp = 0;           
		   if (ioctl(fb,FLCD_SET_SPECIAL_FB,&tmp) < 0) 
		   {
	        	printf("LCD Error: cannot set frame buffer as %d.\n",tmp);
	        	exit(3);
    	   } 
		   break;
        }
		if(capture_trigger(&gb_buf) < 0)
		    continue;
		if(capture_sync(&frame_sync )<0)
		{
		    printf("wait sync failure\n");
		    continue;
		}
		if (fb_mode == FLCD_MODE_RGB){
			show_lcd_rgb(fb_mem, video_buf_virt+gb_buffers.offsets[frame_sync], cap_width, cap_height, vinfo.xres, vinfo.yres);
		}
		else{
			if(cap_id ==0){
			    show_lcd(fb_mem, video_buf_virt+gb_buffers.offsets[frame_sync], cap_width, cap_height, vinfo.xres, vinfo.yres);
				usleep(10);
			} else{
			    show_lcd_352x240_l(fb_mem, video_buf_virt+gb_buffers.offsets[frame_sync], cap_width, cap_height, vinfo.xres, vinfo.yres);
				usleep(10);
			}
//	        usleep(10);

		}

		length++;
		cur_time = av_gettime_from_startup();
		if(cur_time < 10000000){
			printf("\r%dfs               ", length);fflush(stdout);
		}else if(!(length%15)){
			float fps;
			fps = (float)length/cur_time;
			
			printf("\r%dfs, %4.1ffps     ",getpid() , length ,fps*1000000);fflush(stdout);
			if(cap_id ==0){
				printf("\r <%dfs, %4.1ffps>     ",length ,fps*1000000);fflush(stdout);
			}
			else{
				printf("\r\t\t\t\t <%dfs, %4.1ffps>     ", length ,fps*1000000);fflush(stdout);
			}
		}
    }
    printf("\n%d frames done!\n",length);
    close(video_fd);
    close(fb);
}

#define MIN(A,B)	((A)<(B)?(A):(B))
#define MAX(A,B)	((A)>(B)?(A):(B))
#define MK_RGB555(R,G,B)	((MAX(0,MIN(255, R)) << 7) & 0x7c00) | \
							((MAX(0,MIN(255, G)) << 2) & 0x03e0) | \
							((MAX(0,MIN(255, B)) >> 3) & 0x001f)

void
yv12_to_rgb555_rgb555_c(uint8_t * dst,
				 int dst_stride,
				 uint8_t * y_src,
				 uint8_t * u_src,
				 uint8_t * v_src,
				 int y_stride,
				 int uv_stride,
				 int width,
				 int height)
{
	const uint32_t dst_dif = 4 * dst_stride - 2 * width;
	int32_t y_dif = 2 * y_stride - width;

	uint8_t *dst2 = dst + 2 * dst_stride;
	uint8_t *y_src2 = y_src + y_stride;
	uint32_t x, y;

	if (height < 0) {
		height = -height;
		y_src += (height - 1) * y_stride;
		y_src2 = y_src - y_stride;
		u_src += (height / 2 - 1) * uv_stride;
		v_src += (height / 2 - 1) * uv_stride;
		y_dif = -width - 2 * y_stride;
		uv_stride = -uv_stride;
	}

	for (y = height / 2; y; y--) {
		int r, g, b;
		int r2, g2, b2;

		r = g = b = 0;
		r2 = g2 = b2 = 0;

		// process one 2x2 block per iteration
		for (x = 0; x < (uint32_t) width / 2; x++) {
			int u, v;
			int b_u, g_uv, r_v, rgb_y;

			u = u_src[x];
			v = v_src[x];

			b_u = B_U_tab[u];
			g_uv = G_U_tab[u] + G_V_tab[v];
			r_v = R_V_tab[v];
//Y0,U,V=>R0
			rgb_y = RGB_Y_tab[*y_src];
			b = ((rgb_y + b_u) >> 7);
			g = ((rgb_y - g_uv) >> 7);
			r = ((rgb_y + r_v) >> 7);
			*(uint16_t *) dst = MK_RGB555(r, g, b);
//Y1,U,V=>R1
			y_src++;
			rgb_y = RGB_Y_tab[*y_src];
			b = ((rgb_y + b_u) >> 7);
			g = ((rgb_y - g_uv) >> 7);
			r = ((rgb_y + r_v) >> 7);
			*(uint16_t *) (dst + 2) = MK_RGB555(r, g, b);
			y_src++;
//Y2,U,V=>R2
			rgb_y = RGB_Y_tab[*y_src2];
			b2 = ((rgb_y + b_u) >> 7);
			g2 = ((rgb_y - g_uv) >> 7);
			r2 = ((rgb_y + r_v) >> 7);
			*(uint16_t *) (dst2) = MK_RGB555(r2, g2, b2);
			y_src2++;
//Y3,U,V=>R3
			rgb_y = RGB_Y_tab[*y_src2];
			b2 = ((rgb_y + b_u) >> 7);
			g2 = ((rgb_y - g_uv) >> 7);
			r2 = ((rgb_y + r_v) >> 7);
			*(uint16_t *) (dst2 + 2) = MK_RGB555(r2, g2, b2);
			y_src2++;

			dst += 4;
			dst2 += 4;
		}

		dst += dst_dif;
		dst2 += dst_dif;

		y_src += y_dif;
		y_src2 += y_dif;

		u_src += uv_stride;
		v_src += uv_stride;
	}
}
/*
main(int argc, char *argv[])
{
      printf("Press CTRL-C to escape!\n");
    do_focus(CAPTURE_WIDTH,CAPTURE_HEIGHT);
}
*/
void show_lcd(unsigned char *fb_mem, unsigned char *yuv, int cap_width, int cap_height, int lcd_width, int lcd_height)
{
    int             i,j;
    int				width, height;
	int				lcd_y_size = (((lcd_width*lcd_height)+0xffff)&0xffff0000);
//    unsigned char   r_y,g_u,b_v;
unsigned char u_src[1],v_src[1], *k;
     	  
    //printf("fb_mem=0x%x\n",fb_mem);fflush(stdout);
    //r_y=0, g_u=0; b_v=255;
    //rgb2ycbcr(&r_y,&g_u,&b_v);
	if (cap_width == lcd_width) {
		if (cap_height == lcd_height) {
#ifdef VIDEO_MODE_422
		memcpy (fb_mem, yuv, cap_width * cap_height * 2);
		//printf("1fb_mem = %x size = %x\n",fb_mem,cap_width * cap_height);
#else		
       	  	/* Y*/
		memcpy (fb_mem, yuv, cap_width * cap_height);
          	/* UV*/
          	memcpy (fb_mem + lcd_y_size, yuv + cap_width * cap_height, cap_width * cap_height / 4);
          	memcpy (fb_mem + lcd_y_size * 5 / 4, yuv + cap_width * cap_height * 5 / 4, cap_width * cap_height/ 4);
#endif          
  		}
  		else {
  		  if (cap_height < lcd_height)
		    height = cap_height;
		  else
		    height = lcd_height;
		    
#ifdef VIDEO_MODE_422
		memcpy (fb_mem, yuv, cap_width * height * 2);
#else			    
   	      	/* Y*/
		memcpy (fb_mem, yuv, cap_width * height);
          	/* UV*/
          	memcpy (fb_mem + lcd_y_size, yuv + cap_width * cap_height, cap_width * height / 4);
          	memcpy (fb_mem + lcd_y_size * 5 / 4, yuv + cap_width * cap_height * 5 / 4, cap_width * height/ 4);
#endif           	
  		}
    }
  	else {
  		if (cap_height < lcd_height)
		    height = cap_height;
		  else
		    height = lcd_height;
  		if (cap_width < lcd_width)
		    width = cap_width;
		  else
		    width = lcd_width;
		    
#ifdef VIDEO_MODE_422	
#if 0		
		for (i = 0; i < height; i ++)
		{		
		  for (j = 0; j < width*2; j += 4)
		  {
		  *u_src = *(yuv + j);
		  *v_src = *(yuv + j + 2);
		  
		  *(fb_mem + j) = *(yuv + j + 1);
		  *(fb_mem + j + 1) = *u_src;
		  *(fb_mem + j + 2) = *(yuv + j + 3);
		  *(fb_mem + j + 3) = *v_src;
		  }
		  fb_mem += lcd_width * 2;
		  yuv += cap_width*2;
		}	
#else
		if (width == 640)
		for (i = 0; i < height; i ++)	  
		  memcpy (fb_mem + i * lcd_width*2, yuv + i * cap_width*2, width*2);		
		else
		for (i = 0; i < height; i ++)	  
		  memcpy (fb_mem + i * lcd_width*2, yuv + i * cap_width*2 + 1, width*2);			  
#endif
		  
#else	    
   	    	/* Y*/
		for (i = 0; i < height; i ++)
		  memcpy (fb_mem + i * lcd_width, yuv + i * cap_width, width);
	        /* U*/
	        fb_mem += lcd_y_size;
	        yuv += ((cap_width + 15) >> 4) * ((cap_height + 15) >> 4) * 256;//cap_width * cap_height;
		for (i = 0; i < height / 2; i ++)
          	  memcpy (fb_mem + i * lcd_width / 2, yuv + i * cap_width / 2, width / 2);
	        /* V*/
	        fb_mem += lcd_y_size/4;
	        yuv += ((cap_width + 15) >> 4) * ((cap_height + 15) >> 4) * 256 / 4;//cap_width * cap_height/4;
		for (i = 0; i < height / 2; i ++)
          	  memcpy (fb_mem + i * lcd_width / 2, yuv + i * cap_width / 2, width / 2);
#endif 			
	}

}
void show_lcd_352x240_l(unsigned char *fb_mem, unsigned char *yuv, int cap_width, int cap_height, int lcd_width, int lcd_height)
{
    int             i,j;
    int				width, height;
	int				lcd_y_size = (((lcd_width*lcd_height)+0xffff)&0xffff0000);
unsigned char u_src[1],v_src[1], *k;
     	  
	if (cap_height < lcd_height)
		height = cap_height;
	else
		height = lcd_height;
	if (cap_width < lcd_width)
		width = cap_width;
	else
		width = lcd_width;

		/* Y*/
	for (i = 0; i < height; i ++)
		memcpy ((lcd_width/2) + (fb_mem + i * lcd_width), yuv + i * cap_width, width);
	/* U*/
	fb_mem += lcd_y_size;
	yuv += cap_width * cap_height;
	for (i = 0; i < height / 2; i ++)
		memcpy ((lcd_width/4) + (fb_mem + i * lcd_width / 2), yuv + i * cap_width / 2, width / 2);
	/* V*/
	fb_mem += lcd_y_size/4;
	yuv += cap_width * cap_height/4;
	for (i = 0; i < height / 2; i ++)
		memcpy ((lcd_width/4) + (fb_mem + i * lcd_width / 2), yuv + i * cap_width / 2, width / 2);
}

void show_lcd_rgb(unsigned char *fb_mem, unsigned char *yuv, int cap_width, int cap_height, int lcd_width, int lcd_height)
{
    int				width, height;

  	if (cap_height < lcd_height)
	    height = cap_height;
	else
	    height = lcd_height;
  	if (cap_width < lcd_width)
	    width = cap_width;
	else
	    width = lcd_width;
	    
//printf("fb_mem=%x %x %x %x\n",fb_mem,yuv,yuv + cap_width * cap_height,yuv + cap_width * cap_height * 5 / 4);	    
    yv12_to_rgb555_rgb555_c(fb_mem,                                           // dst
                            lcd_width,                                        // dst stride
                            yuv,                                              // y_src
                            yuv + cap_width * cap_height,                     // u_src
                            yuv + cap_width * cap_height * 5 / 4,             // v_src
                            cap_width,                                        // y_stride
                            cap_width/2,                                      // uv stride
                            width,                                            // width
                            height);                                          // height
}
